#include "./verlet.h"

static void mds_verlet_predict( mds_frame *frame,  const mds_verlet_data *data )
{
	assert( frame );
	assert( data );
	assert( frame->sets );
	assert( frame->num_sets >= 3 );
	assert( frame->num_particles > 0 );
	{
		mds_vector*          position = frame->sets[MDS_POSITION];
		mds_vector*          velocity = frame->sets[MDS_VELOCITY];
		const mds_vector*    accel    = frame->sets[MDS_ACCELERATION];
		const Real           half_dt  = data->half_dt;
		const Real           dt       = data->dt;
		mds_pbc_proc         pbc      = frame->pbc_for_r;
		register size_t      i;
		
		for( i=0; i < frame->num_particles; ++i )
		{
			const mds_vector *a = &accel[i];
			mds_vector       *v = &velocity[i];
			mds_vector       *r = &position[i];
			mds_vector_muladd(v, half_dt, a);
			mds_vector_muladd(r, dt,      v);
			pbc( r, & frame->box );
		}
		
	}
}

static void mds_verlet_correct( mds_frame *frame,  const mds_verlet_data *data )
{
	assert( frame );
	assert( data );
	assert( frame->sets );
	assert( frame->num_sets >= 3 );
	assert( frame->num_particles > 0 );
	{
		mds_vector_set       velocity = frame->sets[MDS_VELOCITY];
		const mds_vector_set accel    = frame->sets[MDS_ACCELERATION];
		const Real           half_dt  = data->half_dt;
		Real                 K        = 0;
		size_t i;
		
		for( i=0; i < frame->num_particles; ++i )
		{
			const mds_vector *a = &accel[i];
			mds_vector       *v = &velocity[i];
			mds_vector_muladd(v, half_dt, a);
			K += (v->info.mass) * mds_vector_normsq( v );
		}
		
		frame->K = MDS_F(0.5)*K;
	}
	
}

static void mds_verlet_init_dt( mds_frame *frame, Real dt, mds_verlet_data *data )
{
#if defined(__INTEL_COMPILER)
#	pragma warning ( disable : 869 )
#endif
	assert( data );
	fprintf( stderr, "mds_verlet_init dt=%g\n", dt );
	data->dt      = dt;
	data->half_dt = MDS_F(0.5) * dt;
	fprintf( stderr, "\t dt   = %g\n", data->dt);
	fprintf( stderr, "\t half = %g\n", data->half_dt);
}


void mds_verlet_step_load( mds_step *step, mds_verlet_data *data )
{
	assert( step );
	memset( step, 0, sizeof(mds_step) );
	step->data     = data;
	step->predict  = (mds_step_upgrade_proc) mds_verlet_predict;
	step->correct  = (mds_step_upgrade_proc) mds_verlet_correct;
	step->init_dt  = (mds_step_init_dt_proc) mds_verlet_init_dt;
	step->num_sets = MDS_NUM_SETS_FOR_VERLET; 
}
